import java.util.EventObject;

import javax.swing.JOptionPane;

import lejos.*;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.remote.NXTCommand;
import lejos.pc.comm.NXTCommandConnector;
import lejos.pc.comm.NXTConnector;

public class SMR implements VariableChangeListener{
	private final int NORTE 	= 1;
	private final int OESTE 	= 2;
	private final int SUR 		= 3;
	private final int ESTE 		= 4;
	private final int PING 		= 22;
	private final int PINGM		= 10;
	private final int DIST 		= 1150;
	private final int DISTC		= 35;
	private final int ROTACION 	= 510;
	private final int ROTSENSOR	= 100;
	
	private int izq;
	private int der;
	private int corre;
	private int frente;
	
	private CA ambiente;
	private NXTConnector con;
	private UltrasonicSensor sensor;
	
	public SMR(SMJ joy, CA ambiente,NXTConnector con){
		this.ambiente = ambiente;
		joy.addVariableNumberChangeListener(this);
		this.con = con;
		NXTCommandConnector.setNXTCommand(new NXTCommand(this.con.getNXTComm()));
	    sensor = new UltrasonicSensor(SensorPort.S1);
	}
	
	public void variableChanged(EventObject evt) {
		switch(evt.getSource().toString()){
			case "0":
				this.pingBoton();
				this.correccionMov();
				break;

			case "1":
				this.rotarDer90();
				this.pingDistancias();
				correccionMov();
				break;
				
			case "2":
				this.rotarIzq90();
				this.pingDistancias();
				correccionMov();
				break;
				
			case "3": 
				this.avanzar();
				this.pingDistancias();
				this.correccionMov();
				break;
		}
		/*System.out.println("-------------------------");
		for(int x=0;x<ambiente.getMapa().length;x++){
			for(int y=0;y<ambiente.getMapa().length;y++){
				System.out.print(ambiente.getMapa()[x][y]);
				System.out.print(" ");
			}
			System.out.println(" ");
		}*/
	}

	public void avanzar(){
		System.out.println("Quiero avanzar");
		int mapa [][] = ambiente.getMapa();
		int x = ambiente.getPosX();
		int y = ambiente.getPosY();
		switch(mapa[x][y]){
			case OESTE:
				if(mapa[x+1][y]==1){
					JOptionPane.showMessageDialog(null, "No puedo avanzar, hay una pared en frente.");
					break;
				}
				if(!ambiente.setMapa(x+2, y, OESTE))break;
				ambiente.setMapa(x, y, 0);
				ambiente.setPosX(x+2);
				Motor.A.rotate(DIST, true);
				Motor.B.rotate(DIST, true);
				pingMov();
				break;
				
			case ESTE:
				if(mapa[x-1][y]==1){
					JOptionPane.showMessageDialog(null, "No puedo avanzar, hay una pared en frente.");
					break;
				}
				if(!ambiente.setMapa(x-2, y, ESTE))break;
				ambiente.setMapa(x, y, 0);
				ambiente.setPosX(x-2);
				Motor.A.rotate(DIST, true);
				Motor.B.rotate(DIST, true);
				pingMov();
				break;
				
			case NORTE:
				if(mapa[x][y-1]==1){
					JOptionPane.showMessageDialog(null, "No puedo avanzar, hay una pared en frente.");
					break;
				}
				if(!ambiente.setMapa(x, y-2, NORTE))break;
				ambiente.setMapa(x, y, 0);
				ambiente.setPosY(y-2);
				Motor.A.rotate(DIST, true);
				Motor.B.rotate(DIST, true);
				pingMov();
				break;
				
			case SUR:
				if(mapa[x][y+1]==1){
					JOptionPane.showMessageDialog(null, "No puedo avanzar, hay una pared en frente.");
					break;
				}
				if(!ambiente.setMapa(x, y+2, SUR))break;
				ambiente.setMapa(x, y, 0);
				ambiente.setPosY(y+2);
				Motor.A.rotate(DIST, true);
				Motor.B.rotate(DIST, true);
				pingMov();
				break;
				
		}
	}
	
	public void rotarDer90(){
		Motor.A.rotate(ROTACION,true); //true hace que la llamada no sea bloqueante
	    Motor.B.rotate(-ROTACION);
	    int mapa [][] = ambiente.getMapa();
		int x = ambiente.getPosX();
		int y = ambiente.getPosY();
		switch(mapa[x][y]){
			case OESTE:
				ambiente.setMapa(x, y, SUR);
				break;
			case ESTE:
				ambiente.setMapa(x, y, NORTE);
				break;
			case NORTE:
				ambiente.setMapa(x, y, OESTE);
				break;
			case SUR:
				ambiente.setMapa(x, y, ESTE);
				break;
		}
	}
	
	public void rotarIzq90(){
		Motor.B.rotate(ROTACION,true); //true hace que la llamada no sea bloqueante
	    Motor.A.rotate(-ROTACION);
	    int mapa [][] = ambiente.getMapa();
		int x = ambiente.getPosX();
		int y = ambiente.getPosY();
		switch(mapa[x][y]){
			case OESTE:
				ambiente.setMapa(x, y, NORTE);
				break;
			case ESTE:
				ambiente.setMapa(x, y, SUR);
				break;
			case NORTE:
				ambiente.setMapa(x, y, ESTE);
				break;
			case SUR:
				ambiente.setMapa(x, y, OESTE);
				break;
		}
	}
	
	private int pingIzq(){
		Motor.C.rotate(ROTSENSOR);
		try {Thread.sleep(5);} catch (InterruptedException e) {}
		boolean aux = ((izq = sensor.getDistance()) < PING);
		System.out.println("Izq = "+izq);
		Motor.C.rotate(-ROTSENSOR);
		if(aux)return 1;
		return 0;
	}

	private int pingFrente(){
		 if((frente = sensor.getDistance()) < PING)return 1;
		 return 0;
	}
	
	private int pingDer(){
		Motor.C.rotate(-ROTSENSOR);
		try {Thread.sleep(5);} catch (InterruptedException e) {}
		boolean aux = ((der = sensor.getDistance()) < PING);
		System.out.println("Der = "+der);
		Motor.C.rotate(ROTSENSOR);
		if(aux)return 1;
		return 0;
	}
	
	private void pingBoton(){
		System.out.println("TIro ping con boton");
		int mapa [][] = ambiente.getMapa();
		int x = ambiente.getPosX();
		int y = ambiente.getPosY();
		switch(mapa[x][y]){
			case OESTE:
				ambiente.setMapa(x+1, y, this.pingFrente());
				ambiente.setMapa(x, y+1, this.pingDer());
				ambiente.setMapa(x, y-1, this.pingIzq());
				break;
			case ESTE:
				ambiente.setMapa(x-1, y, this.pingFrente());
				ambiente.setMapa(x, y-1, this.pingDer());
				ambiente.setMapa(x, y+1, this.pingIzq());
				break;
			case NORTE:
				ambiente.setMapa(x, y+1, this.pingFrente());
				ambiente.setMapa(x+1, y, this.pingDer());
				ambiente.setMapa(x-1, y, this.pingIzq());
				break;
			case SUR:
				ambiente.setMapa(x, y+1, this.pingFrente());
				ambiente.setMapa(x-1, y, this.pingDer());
				ambiente.setMapa(x+1, y, this.pingIzq());
				break;
		}		
	}
	
	public void pingDistancias(){
		System.out.println("Tiro ping distancias");
		int mapa [][] = ambiente.getMapa();
		int x = ambiente.getPosX();
		int y = ambiente.getPosY();
		switch(mapa[x][y]){
			case OESTE:
				/*if(mapa[x+1][y] == 0)*/ ambiente.setMapa(x+1, y, this.pingFrente());
				/*if(mapa[x][y+1] == 0)*/ ambiente.setMapa(x, y+1, this.pingDer());
				/*if(mapa[x][y-1] == 0)*/ ambiente.setMapa(x, y-1, this.pingIzq());
				break;
			case ESTE:
				/*if(mapa[x-1][y] == 0)*/ ambiente.setMapa(x-1, y, this.pingFrente());
				/*if(mapa[x][y-1] == 0)*/ ambiente.setMapa(x, y-1, this.pingDer());
				/*if(mapa[x][y+1] == 0)*/ ambiente.setMapa(x, y+1, this.pingIzq());
				break;
			case NORTE:
				/*if(mapa[x][y+1] == 0)*/ ambiente.setMapa(x, y+1, this.pingFrente());
				/*if(mapa[x+1][y] == 0)*/ ambiente.setMapa(x+1, y, this.pingDer());
				/*if(mapa[x-1][y] == 0)*/ ambiente.setMapa(x-1, y, this.pingIzq());
				break;
			case SUR:
				/*if(mapa[x][y+1] == 0)*/ ambiente.setMapa(x, y+1, this.pingFrente());
				/*if(mapa[x-1][y] == 0)*/ ambiente.setMapa(x-1, y, this.pingDer());
				/*if(mapa[x+1][y] == 0)*/ ambiente.setMapa(x+1, y, this.pingIzq());
				break;
		}
	}

	public CA getAmbiente(){
		return ambiente;
	}
	
	private void pingMov(){
		while(Motor.A.isMoving()){
			if(sensor.getDistance() <= PINGM){
				Motor.A.stop();
				Motor.B.stop();
				break;
			}
		}
	}
	
	private void correccionMov(){
		if((izq<DISTC)&&(der<DISTC)){
			corre = (izq + der)/2;
			if(izq < der){
				Motor.A.rotate(6*(der-corre), true); 
				Motor.B.rotate(6*(-(der-corre)));
			}
			else{
				Motor.B.rotate(6*(izq-corre), true); 
				Motor.A.rotate(6*(-(izq-corre)));
			}
			
			if(frente <= 10){
				Motor.B.rotate(-90, true); 
				Motor.A.rotate(-90, true);
			}
			if((frente >= 15)&&(frente <= 20)){
				Motor.B.rotate(90, true); 
				Motor.A.rotate(90, true);
			}
		}		
	}
}
